// --------------------------------------------------------------------------------------------------------------------
// <copyright file="NavigationMetrics.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Test.Robotics.Metrics;
    using Test.Robotics.Navigation;

    /// <summary>
    /// Class for calculating various navigation metrics
    /// </summary>
    public class NavigationMetrics
    {
        /// <summary>
        /// Matrix of all the possible distances to travel
        /// </summary>
        private double[][] distances;

        /// <summary>
        /// List of all the absolute ground truth positions of the landmarks
        /// </summary>
        private Dictionary<int, Pose2D> groundTruths = new Dictionary<int, Pose2D>();

        /// <summary>
        /// Identifier for each run
        /// </summary>
        private long runId;

        /// <summary>
        /// Initializes a new instance of the <see cref="NavigationMetrics" /> class.
        /// </summary>
        /// <param name="distancesFile">Path to a serialized matrix of distances</param>
        /// <param name="groundTruthFile">Path to the ground truth file</param>
        /// <param name="runId">A number that identifies this run</param>
        public NavigationMetrics(string distancesFile, string groundTruthFile, long runId)
        {
            this.distances = Serializer.Load<double[][]>(distancesFile, false);
            this.LoadGroundTruths(groundTruthFile);
            this.runId = runId;
        }

        /// <summary>
        /// Generate a localization positional accuracy result.
        /// Localization accuracy compares the ground truth to the position reported by the robot
        /// </summary>
        /// <param name="navDataMessage">The navigation data message</param>
        /// <param name="groundTruthMessage">The ground truth message</param>
        /// <param name="previousLandmarkId">The previous landmark Id</param>
        /// <returns>A metrics result message</returns>       
        public MetricsResultMessage GetLocPositionResult(NavDataMessage navDataMessage, RobotPoseEstimationAgentMessage groundTruthMessage, int previousLandmarkId)
        {
            int duration = navDataMessage.ElapsedSeconds;

            Pose2D navPose = navDataMessage.Pose;
            Pose2D groundTruthPose = groundTruthMessage.RobotPose.Value;

            double dx = (navPose.X / MathConstants.MilliunitsPerUnit) - groundTruthPose.X;
            double dy = (navPose.Y / MathConstants.MilliunitsPerUnit) - groundTruthPose.Y;
            double accuracy = Math.Sqrt((dx * dx) + (dy * dy));
            return new MetricsResultMessage(
                type: "LocPosition",
                groundTruthId: navDataMessage.LandmarkId,
                accuracy: accuracy,
                precision: 0,
                weight: 1,
                debugData: string.Format("start:{0} end:{1}", previousLandmarkId, navDataMessage.LandmarkId),
                duration: duration,
                robot: null,
                runId: this.runId,
                originatingMessage: new AgentMessage(AgentMessage.GetCurrentTime()));
        }

        /// <summary>
        /// Generate a localization orientation accuracy result 
        /// Localization accuracy compares the ground truth to the position reported by the robot
        /// </summary>
        /// <param name="navDataMessage">The navigation data message</param>
        /// <param name="groundTruthMessage">The ground truth message</param>
        /// <returns>A metrics result message</returns>       
        public MetricsResultMessage GetLocOrientationResult(NavDataMessage navDataMessage, RobotPoseEstimationAgentMessage groundTruthMessage)
        {
            int duration = navDataMessage.ElapsedSeconds;

            double accuracy = Math.Abs(navDataMessage.Pose.SmallestCircularDeltaAngle(groundTruthMessage.RobotPose.Value.Heading));
            return new MetricsResultMessage(
                type: "LocOrientation",
                groundTruthId: navDataMessage.LandmarkId,
                accuracy: accuracy,
                precision: 0,
                weight: 1,
                debugData: string.Empty,
                duration: duration,
                robot: null,
                runId: this.runId,
                originatingMessage: new AgentMessage(AgentMessage.GetCurrentTime()));
        }

        /// <summary>
        /// Generate a navigation positional accuracy result 
        /// Navigation accuracy compares the current ground truth to the destination waypoint pose
        /// </summary>
        /// <param name="navDataMessage">The navigation data message</param>
        /// <param name="groundTruthMessage">The ground truth message</param>
        /// <param name="previousLandmarkId">The previous landmark Id</param>
        /// <returns>A metrics result message</returns>       
        public MetricsResultMessage GetNavPositionResult(NavDataMessage navDataMessage, RobotPoseEstimationAgentMessage groundTruthMessage, int previousLandmarkId)
        {
            int duration = navDataMessage.ElapsedSeconds;

            Pose2D waypointPose = this.groundTruths[navDataMessage.LandmarkId];
            Pose2D groundTruthPose = groundTruthMessage.RobotPose.Value;

            double dx = waypointPose.X - groundTruthPose.X;
            double dy = waypointPose.Y - groundTruthPose.Y;
            double accuracy = Math.Sqrt((dx * dx) + (dy * dy));
            return new MetricsResultMessage(
                type: "NavPosition",
                groundTruthId: navDataMessage.LandmarkId,
                accuracy: accuracy,
                precision: 0,
                weight: 1,
                debugData: string.Format("start:{0} end:{1}", previousLandmarkId, navDataMessage.LandmarkId),
                duration: duration,
                robot: null,
                runId: this.runId,
                originatingMessage: new AgentMessage(AgentMessage.GetCurrentTime()));
        }

        /// <summary>
        /// Generate a navigation orientation accuracy result.
        /// Navigation accuracy compares the current ground truth to the destination waypoint pose
        /// </summary>
        /// <param name="navDataMessage">The navigation data message</param>
        /// <param name="groundTruthMessage">The ground truth message</param>
        /// <returns>A metrics result message</returns>       
        public MetricsResultMessage GetNavOrientationResult(NavDataMessage navDataMessage, RobotPoseEstimationAgentMessage groundTruthMessage)
        {
            int duration = navDataMessage.ElapsedSeconds;

            Pose2D waypointPose = this.groundTruths[navDataMessage.LandmarkId];

            double accuracy = Math.Abs(waypointPose.SmallestCircularDeltaAngle(groundTruthMessage.RobotPose.Value.Heading));
            return new MetricsResultMessage(
                type: "NavOrientation",
                groundTruthId: navDataMessage.LandmarkId,
                accuracy: accuracy,
                precision: 0,
                weight: 1,
                debugData: string.Empty,
                duration: duration,
                robot: null,
                runId: this.runId,
                originatingMessage: new AgentMessage(AgentMessage.GetCurrentTime()));
        }

        /// <summary>
        /// Generate a navigation speed result 
        /// </summary>
        /// <param name="navDataMessage">The navigation data message</param>
        /// <param name="previousLandmarkId">The previous landmark Id</param>
        /// <returns>A metrics result message</returns>       
        public MetricsResultMessage GetNavSpeedResult(NavDataMessage navDataMessage, int previousLandmarkId)
        {
            Pose2D navPose = navDataMessage.Pose;

            // speed accuracy is the duration / distance (inverse of the speed)
            int duration = navDataMessage.ElapsedSeconds;

            double distance = this.distances[previousLandmarkId][navDataMessage.LandmarkId];

            return new MetricsResultMessage(
                type: "NavSpeed",
                groundTruthId: navDataMessage.LandmarkId,
                accuracy: duration / distance,
                precision: 0,
                weight: distance,
                debugData: string.Empty,
                duration: duration,
                robot: null,
                runId: this.runId,
                originatingMessage: new AgentMessage(AgentMessage.GetCurrentTime()));
        }

        /// <summary>
        /// Loads the ground truths for the current SLAM map.
        /// There are the actual poses the robot had at each landmark that are used as waypoints during testing.
        /// </summary>
        /// <param name="filePath">Path to the ground truth file</param>
        private void LoadGroundTruths(string filePath)
        {
            using (StreamReader fileReader = new StreamReader(filePath))
            {
                while (!fileReader.EndOfStream)
                {
                    string line = fileReader.ReadLine();
                    if (string.IsNullOrEmpty(line) || line[0] == '#')
                    {
                        continue;
                    }

                    string[] parts = line.Split();
                    int landmarkId = int.Parse(parts[0]);
                    double x = double.Parse(parts[1]);
                    double y = double.Parse(parts[2]);
                    double heading = double.Parse(parts[3]);

                    // the absolute ground truth file may contain the same id more than once 
                    // for example when the same marker is the beginning and end of a loop.
                    // The waypoint navigation runs will only use the first instance of each, so we do the same here.
                    if (!this.groundTruths.ContainsKey(landmarkId))
                    {
                        this.groundTruths.Add(landmarkId, new Pose2D(x, y, heading));
                    }
                }
            }
        }
    }
}
